%author: wxj233
%time: 2023.10.25 18:00
%function: 仿真器

classdef Simulater < handle
    %SIMULATE 用于完成仿真相关操作
    %   根据传入的最早时间，时间差，以及生成的特征点分布，目标初始位置，速度，加速度等产生轨迹
    
    properties
        dt  % 时间间隔
        gstartTime  % 全局起始时间
        gendTime  % 全局结束时间
        tracks  % 生成的轨迹集合[track1, track2, ...] 单个track中结构为[t, x, y, id; ...]
        timeIndex  % 时间索引
    end
    
    methods
        function obj = Simulater(dt)
            %SIMULATE 构造此类的实例
            %   此处显示详细说明
            obj.dt = dt;
            obj.gstartTime = 0;
            obj.gendTime = 0;
            obj.tracks = {};
            obj.timeIndex = 0;

            obj.getId();  % 每次都进行初始化
        end


        function setTimeIndex(obj, time)
            % 设置时间索引值
            obj.timeIndex = time;
        end


        function [frame, isNextFrame] = getNextFrame(obj)
            % 获取下一帧数据
            % return frame: 返回当前帧，没有则为空
            % return isNextFrame: 是否还有下一帧
            frame = obj.getFrame(obj.timeIndex);
            obj.timeIndex = round(obj.timeIndex + obj.dt, 3);
            if obj.timeIndex > obj.gendTime
                isNextFrame = false;
            else
                isNextFrame = true;
            end
        end


        function frame = getFrame(obj, time, varargin)
            % 获取指定时刻的数据帧
            % time: 指定时刻
            % varargin: 已经生成的轨迹，没传值则使用默认已经生成的全部轨迹
            if size(varargin, 2) == 0
                varargin = obj.tracks;
            end

            frame = [];
            for i = 1:1:size(varargin, 2)
                frame = [frame; varargin{i}(varargin{i}(:,1) == round(time, 3), :)];  % 由于精度问题，这个地方只能取3位小数
            end
        end
        

        function outps = generateNoise(obj, density, area, varargin)
            % 产生噪声，假设噪声在区域内服从均匀分布，噪声出现的个数服从泊松分布
            % density: 噪声密度
            % area: 区域[xmin, xmax, ymin, ymax]
            % varargin:  {seed}为随机种子
            % return outps: [t, x, y, 0; ...] 噪声默认id为0
            if size(varargin, 2) > 0
                rng(varargin{1});  % 指定随机数种子
            end
            
            outps = [];
            dx = area(2)-area(1);
            dy = area(4)-area(3);
            lambda = density*dx*dy;
            times = round(obj.gstartTime : obj.dt : obj.gendTime, 3);
            num = poissrnd(lambda, [1, length(times)]);
            for i = 1:1:length(times)
                if num(i) == 0
                    continue;
                end
                ps = rand(num(i), 2).*[dx, dy] + [area(1), area(3)];
                time = zeros(num(i), 1)+times(i);
                outps = [outps; [time, ps]];
            end
            outps = [outps, zeros(size(outps, 1), 1)];  % 噪声默认id为0

            obj.tracks = [obj.tracks, outps];
        end


        function outps = generateTrack(obj, cluster, point0, v, a, startTime, endTime, id, sd, varargin)
            % 产生仿真轨迹
            % cluster: 当前目标特征点[x1, y1; x2, y2; ...]
            % point0: 初始位置[x0, y0], time=0时刻位置
            % v: 初始速度[vx, vy]
            % a: 初始加速度[ax, ay]
            % startTime: 采样开始时间
            % endTime: 采样结束时间
            % id: 该条轨迹的标识
            % sd: 标准差
            % varargin: 可变参数,随机数种子
            % return oups: [t, x, y, id]
            if size(varargin, 2) > 0
                rng(varargin{1});  % 设定随机数种子
            end
            obj.gstartTime = min([obj.gstartTime, startTime]);
            obj.timeIndex = obj.gstartTime;
            obj.gendTime = max([obj.gendTime, endTime]);
            fuPNum = size(cluster, 1);  % 特征点数量
            times = round(startTime:obj.dt:endTime, 3);  % 保留3位小数, 不然精度就有问题
            ids = id*ones([length(times), 1]);  % 点的id
            isShow = rand(length(times), fuPNum);  % 用于判断每一帧特征点是否有效
        
            points = point0 + v.*times' + 0.5*a.*(times.*times)';  % 中心点时序位置
            ftracks = [];  % feature tracks
            for j = 1:1:fuPNum
                if size(varargin, 2) > 1
                    rng(varargin{2}+j);  % 设定正态分布随机数种子
                end
                fps = points + cluster(j, 1:2) + normrnd(0, sd, [length(times), 2]);
                ftracks = cat(3, ftracks, [times', fps, ids+j]);
%                 ftracks = cat(3, ftracks, [times', fps, ids]);
            end

            outps = [];
            for i = 1:1:length(times)
                for j = 1:1:fuPNum
                    if isShow(i, j)<cluster(j, 3)
                        p = ftracks(i, :, j);
                        outps = [outps; p];
                    end
                end
            end
            obj.tracks = [obj.tracks, outps];
        end
        
        
        function cluster = generateFuturePoints(~, len, width, probabilitys, varargin)
            % 用于产生特征点，在区域内均匀分布
            % length, width: 要生成特征点的区域长宽（区域是任意区域，这个地方只是为了仿真而使用矩形区域）
            % probabilitys: 每个特征点出现的概率值[p1, p2, ...]，关键点的数量与概率值的数量一一对应
            % varargin: {seed}随机数种子用于程序复现,省略时则为不可复现的随机数
            % return: 返回各个特征点相对于中心点（长宽的中心）的位置[x1, y1, p1; x2, y2, p2; ...]
            if size(varargin, 2) > 0
                rng(varargin{1});  % 设定随机数种子
            end
            num = length(probabilitys);
            cluster = rand(num, 2).*[width, len] - 0.5*[width, len];
            cluster = [cluster, probabilitys'];
        end
    end


    methods (Static)
        function id = getId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
                ID = varargin{1};
                return;
          end

          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end
end

